#include <string.h>
#include "LED_Test.h"
#include <util/delay.h>
#include "os.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include "kernel.h"

//Comment out the following line to remove debugging code from compiled version.
//#define DEBUG

/*===========
  * RTOS Internal
  *===========
  */

extern void CSwitch();
extern void Exit_Kernel();    /* this is the same as CSwitch() */

/* Prototype */
void Task_Terminate(void);


extern void Enter_Kernel();



/******************************************************************************
  * This table contains ALL process descriptors. It doesn't matter what       *
  * state a task is in.													      *
  *****************************************************************************/
volatile static PROCESS_DESCRIPTOR Process[MAXPROCESS];
volatile static TASK_HANDLER Handlers[MAXPROCESS+1]; //plus one for idle task

/******************************************************************************
  * The process descriptor of the currently RUNNING task.					  *
  *****************************************************************************/
volatile static PROCESS_DESCRIPTOR* CurrentProcess;


/************************************************************************
  * This pointer contains the next SYSTEM process to be executed
  ***********************************************************************/
volatile static TASK_HANDLER *SYS_Next;
volatile static TASK_HANDLER *SYS_Next;

/************************************************************************
  * The process descriptor of the currently RUNNING task.
  ***********************************************************************/
volatile static TASK_HANDLER* curTask;



/**
  * (Note: This and the following stack pointers are used primarily by the
  *   context switching code, i.e., CSwitch(), which is written in assembly
  *   language.)
  */
volatile unsigned char *KernelSp;

/**
  * This is a "shadow" copy of the stack pointer of "Cp", the currently
  * running task. During context switching, we need to save and restore
  * it into the appropriate process descriptor.
  */
volatile unsigned char *CurrentSp;


/** 1 if kernel has been started; 0 otherwise. */
volatile static unsigned int KernelActive;

/** number of tasks created so far */
volatile static unsigned int Tasks;

/**
 * When creating a new task, it is important to initialize its stack just like
 * it has called "Enter_Kernel()"; so that when we switch to it later, we
 * can just restore its execution context on its stack.
 * (See file "cswitch.S" for details.)
 */

void Kernel_Create_Task_At( PROCESS_DESCRIPTOR *process, voidfuncptr f )
{
   unsigned char *sp;

#ifdef DEBUG
   int counter = 0;
#endif

   //Changed -2 to -1 to fix off by one error.
   sp = (unsigned char *) &(process->workSpace[WORKSPACE-1]);



   /*----BEGIN of NEW CODE----*/
   //Initialize the workspace (i.e., stack) and PROCESS_DESCRIPTOR here!

   //Clear the contents of the workspace
   memset(&(process->workSpace),0,WORKSPACE);

   //Store terminate at the bottom of stack to protect against stack underrun.
   *(unsigned char *)sp-- = ((unsigned int)Task_Terminate) & 0xff;
   *(unsigned char *)sp-- = (((unsigned int)Task_Terminate) >> 8) & 0xff;

   //Place return address of function at bottom of stack
   *(unsigned char *)sp-- = ((unsigned int)f) & 0xff;
   *(unsigned char *)sp-- = (((unsigned int)f) >> 8) & 0xff;

#ifdef DEBUG
   //Fill stack with initial values for development debugging
   //Registers 0 -> 31 and the status register
   for (counter = 0; counter < 33; counter++)
   {
      *(unsigned char *)sp-- = counter;
   }
#else
   //Place stack pointer at top of stack
   sp = sp - 33;
#endif

   process->sp = sp;		/* stack pointer into the "workSpace" */
   process->code = f;		/* function to be executed as a task */
   process->request = NONE;

   /*----END of NEW CODE----*/

   process->state = READY;

}

/**
  *  Create a new task
  */
static void Kernel_Create_Task( voidfuncptr f )
{
   int x;

   if (Tasks == MAXPROCESS) return;  /* Too many task! */

   /* find a DEAD PROCESS_DESCRIPTOR that we can use  */
   for (x = 0; x < MAXPROCESS; x++) {
       if (Process[x].state == DEAD) break;
   }

   ++Tasks;
   Kernel_Create_Task_At( &(Process[x]), f );

}


/**
  * This internal kernel function is a part of the "scheduler". It chooses the
  * next task to run, i.e., Cp.
  */
static void Dispatch()
{
	//unsigned int id = 0;
	int nextTaskLevel=-1;
	//TASK_HANDLER *nextTask=NULL;
	//TASK_HANDLER *nextTaskOwner=NULL;
	int curRequest;
	
	curRequest=(int)curTask->request;


/***********************************************************************************
 * Check to see if what type of task to execute next. 
 ***********************************************************************************/
	if (curTask->level!=SYSTEM || (curTask->level==SYSTEM && curTask->state==DEAD)){
		if (SYS_Next!=NULL){ // Execute System Tasks immediatelly
			nextTaskLevel=SYSTEM;
		}else {
		
			if (curRequest==NEXT){
				if ( curTask->level==RR ){
					nextTaskLevel=RR;
			
				}else if ( curTask->level==PERIODIC ){
					nextTaskLevel = PERIODIC;
			
				}else{
					nextTaskLevel=RR;
				}
			}
		}

	}


 while(Process[nextPPP].state != READY) {
      nextPPP = (nextPPP + 1) % MAXPROCESS;
   }

   CurrentProcess = &(Process[nextPPP]);
   CurrentSp = CurrentProcess->sp;
   CurrentProcess->state = RUNNING;

   nextPPP = (nextPPP + 1) % MAXPROCESS;
}

/**
  * This internal kernel function is the "main" driving loop of this full-served
  * model architecture. Basically, on OS_Start(), the kernel repeatedly
  * requests the next user task's next system call and then invokes the
  * corresponding kernel function on its behalf.
  *
  * This is the main loop of our kernel, called by OS_Start(). - Now called by OS_Init()
  */
static void Next_Kernel_Request()
{
   Dispatch();  /* select a new task to run */

   while(1) {
       CurrentProcess->request = NONE; /* clear its request */

       /* activate this newly selected task */
       CurrentSp = CurrentProcess->sp;
       Exit_Kernel();    /* or CSwitch() */

       /* if this task makes a system call, it will return to here! */

        /* save the Cp's stack pointer */
       CurrentProcess->sp = CurrentSp;

       switch(CurrentProcess->request){
       case CREATE:
           Kernel_Create_Task( CurrentProcess->code );
           break;
       case NEXT:
	   case NONE:
           /* NONE could be caused by a timer interrupt */
          CurrentProcess->state = READY;
          Dispatch();
          break;
       case TERMINATE:
          /* deallocate all resources used by this task */
          CurrentProcess->state = DEAD;
          Dispatch();
          break;
       default:
          /* Houston! we have a problem here! */
          break;
       }
    }
}

 
int Task_Create(void (*f)(void), int arg, unsigned int level, unsigned int name){


	//PROCESS_DESCRIPTOR *pd;
//	TASK_HANDLER *th;
    int isCreated = 0;

//	pd= malloc(sizeof(PROCESS_DESCRIPTOR));
//	memset(pd, 0, sizeof(TASK_HANDLER));
		
	if(KernelActive) {
		Disable_Interrupt();
		CurrentProcess ->level = level;
		CurrentProcess ->name = name;
		CurrentProcess ->arg = arg;
		CurrentProcess ->request = CREATE;
		CurrentProcess ->code = f;
	//	th->process_desc = CurrentProcess;
		Enter_Kernel(); //This is in the assembly code
		isCreated = 1;
	}else  {
	 	Kernel_Create_Task(f);
		isCreated =1;

	}
    
	/* if task create is not successful return 0 */
	return isCreated;


}

/**
  * The calling task gives up its share of the processor voluntarily.
  */
void Task_Next()
{
   if (KernelActive) {
     Disable_Interrupt();
     CurrentProcess ->request = NEXT;
     Enter_Kernel();
  }
}


/**
  * The calling task terminates itself.
  */
void Task_Terminate()
{
   if (KernelActive) {
      Disable_Interrupt();
      CurrentProcess -> request = TERMINATE;
      Enter_Kernel();
     /* never returns here! */
   }
}



/*================
  * RTOS  API  and Stubs
  *================
  */

/**
  * This function initializes the RTOS and must be called before any other
  * system calls.
  */
void OS_Abort() {
    Disable_Interrupt();
   
} // end OS_Abort function


void idle_task(){
	for(;;){}
}


void OS_Init()
{

   nextPPP = 0;

   CLKPR = (1<<CLKPCE); 	//  prescaler bit  1000 0000
   CLKPR = 0x00;	
   int x;

   Tasks = 0;
   KernelActive = 0;
   //NextP = 0;

   //Intialize the LED's
   init_LED_D2();

	
	//Reminder: Clear the memory for the task on creation.
   for (x = 0; x < MAXPROCESS+1; x++) {
      memset(&(Process[x]),0,sizeof(PROCESS_DESCRIPTOR));
      Process[x].state = DEAD;
	  memset(&(Handlers[x]),0,sizeof(TASK_HANDLER));
	  Handlers[x].state = DEAD;
   }

	/*Create an Idle Task */
	Task_Create(idle_task, IDLE, IDLE, IDLE);

   
   /* These tasks should be created inside of main.
    * The only task created in OS_Init should be System tasks or main()
	*/

	Task_Create(main(), 0, SYSTEM, 0);
	Enable_Interrupt();
    /******* OS_Start() Code *******/
   if ( (! KernelActive) && (Tasks > 0)) {
       Disable_Interrupt();
      /* we may have to initialize the interrupt vector for Enter_Kernel() here. */

      /* here we go...  */
      KernelActive = 1;
      Next_Kernel_Request();
      /* NEVER RETURNS!!! */
   }



}
